"""
Parameter	Description
COM_OF_LOSS_T	 5    Time-out (in seconds) to wait when offboard connection is lost before triggering offboard lost failsafe (COM_OBL_RC_ACT)
COM_OBL_RC_ACT	 0    Flight mode to switch to if offboard control is lost (Values are - 0: Position, 1: Altitude, 2: Manual, 3: *Return, 4: *Land*).
COM_RC_OVERRIDE	 3    Controls whether stick movement on a multicopter (or VTOL in MC mode) causes a mode change to Position mode. This is not enabled for offboard mode by default.
COM_RC_STICK_OV	 30   The amount of stick movement that causes a transition to Position mode (if COM_RC_OVERRIDE is enabled).
COM_RCL_EXCEPT	 0    Specify modes in which RC loss is ignored and the failsafe action not triggered. Set bit 2 to ignore RC loss in Offboard mode.
"""



import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleStatus, VehicleOdometry,SensorGps,VehicleGlobalPosition,VehicleLocalPosition


class PxOffboard():
    """Interface for controlling a vehicle in offboard mode."""

    def __init__(self, main_node: Node = None) -> None:
        self.main_node = main_node
        self.id:str = self.main_node.vehicleID
        # if main node has simulation_mode, then set self.prefix to self.id
        self.prefix = ''
        if hasattr(self.main_node, 'uav_simulation_mode'):
            if self.main_node.uav_simulation_mode:
                self.prefix = '/'+self.id


        # Configure QoS profile for publishing and subscribing ??????
        self.qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        # Create publishers
        # 
        self.offboard_control_mode_publisher = self.main_node.create_publisher(
            OffboardControlMode, self.prefix+'/fmu/in/offboard_control_mode', self.qos_profile) 
        self.trajectory_setpoint_publisher = self.main_node.create_publisher(
            TrajectorySetpoint, self.prefix+'/fmu/in/trajectory_setpoint', self.qos_profile)
        self.vehicle_command_publisher = self.main_node.create_publisher(
            VehicleCommand, self.prefix+'/fmu/in/vehicle_command', self.qos_profile)

        # Create subscribers
        self.vehicle_gps_posion_subscriber = self.main_node.create_subscription(
            SensorGps, self.prefix+'/fmu/out/vehicle_gps_position', self.vehicle_gps_position_callback, self.qos_profile)
        self.vehicle_status_subscriber = self.main_node.create_subscription(
            VehicleStatus, self.prefix+'/fmu/out/vehicle_status', self.vehicle_status_callback, self.qos_profile)
        self.vehicle_Odometry_subscriber = self.main_node.create_subscription(
            VehicleOdometry, self.prefix+'/fmu/out/vehicle_odometry', self.vehicle_odometry_callback, self.qos_profile)
        self.vehicle_global_position_subscriber = self.main_node.create_subscription(
            VehicleGlobalPosition, self.prefix+'/fmu/out/vehicle_global_position', self.vehicle_global_position_callback, self.qos_profile)
        self.vehicle_local_position_subscriber = self.main_node.create_subscription(
            VehicleLocalPosition, self.prefix+'/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, self.qos_profile)        
        # Initialize variables
        
        self.vehicle_status = VehicleStatus()       #Topic update time ROS 500ms , Mavlink ?     ,  uorb 500ms
        self.vehicle_odometry=VehicleOdometry()     #Topic update time ROS 8ms   , Mavlink ?     ,  uorb 8ms
        self.vehicle_gps_position = SensorGps()     #Topic update time ROS 8ms   , Mavlink 200ms ,  uorb 125ms
        self.vehicle_global_position = VehicleGlobalPosition()  #Topic update time ROS 10ms   , Mavlink ?,  uorb ?
        self.vehicle_local_position = VehicleLocalPosition()

        # default control_mode 
        self._offboard_control_mode = {'position':True, 'velocity':False, 'acceleration':False, 'attitude':False, 'body_rate':False}
        self.offboard_control_mode_timer = None

        # some variables
        self.last_static_position =[0,0,0]

    def vehicle_local_position_callback(self,vehicle_local_position):
        self.vehicle_local_position = vehicle_local_position
        pass

    def vehicle_global_position_callback(self,vehicle_global_position):
        self.vehicle_global_position = vehicle_global_position
        pass
    
    def vehicle_odometry_callback(self,vehicle_odometry):
        self.vehicle_odometry = vehicle_odometry
        pass

    def vehicle_gps_position_callback(self, vehicle_gps_position):
        """Callback function for vehicle_gps_position topic subscriber."""
        self.vehicle_gps_position = vehicle_gps_position
    
    def vehicle_status_callback(self, vehicle_status):
        """Callback function for vehicle_status topic subscriber."""
        self.vehicle_status = vehicle_status    
      
    # VehicleCommand: arm,disarm, engage_offboard_mode, land
    def arm(self):
        """Send an arm command to the vehicle."""
        self.publish_vehicle_command(
            VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0)
        self.main_node.get_logger().info('Arm command sent')

    def disarm(self):
        """Send a disarm command to the vehicle."""
        self.publish_vehicle_command(
            VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0)
        self.main_node.get_logger().info('Disarm command sent')

    def engage_offboard_mode(self):
        """Switch to offboard mode."""
        self.publish_vehicle_command(
            VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0)
        self.main_node.get_logger().info("Switching to offboard mode")

    def land(self):
        """Switch to land mode."""
        self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND)
        self.main_node.get_logger().info("Switching to land mode")

    def publish_vehicle_command(self, command, **params) -> None:
        """Publish a vehicle command."""
        msg = VehicleCommand()
        msg
        msg.command = command
        msg.param1 = params.get("param1", 0.0)
        msg.param2 = params.get("param2", 0.0)
        msg.param3 = params.get("param3", 0.0)
        msg.param4 = params.get("param4", 0.0)
        msg.param5 = params.get("param5", 0.0)
        msg.param6 = params.get("param6", 0.0)
        msg.param7 = params.get("param7", 0.0)
        # if uav1 then target_component = 1 if uav2 then target_component = 2 depends on the self.id last number
        # get the last number of self.id
        try: 
            msg.target_system = int(self.id[-1])
        except:
            msg.target_system = 1
            print("self.id[-1] is not a number")
        msg.target_component = 1
        msg.source_system = 1
        msg.source_component = 1
        msg.from_external = True
        msg.timestamp = int(self.main_node.get_clock().now().nanoseconds / 1000)
        self.vehicle_command_publisher.publish(msg)
    #VehicleCommand

    #OffboardControlMode
    def start_offboard_control_mode(self):
        # Create a timer to publish control commands
        if self.offboard_control_mode_timer is None:
            self.offboard_control_mode_timer = self.main_node.create_timer(0.1, self.offboard_control_mode_timer_callback)
    #To Do: need to confirm...............
    def stop_offboard_control_mode(self):
        self.offboard_control_mode_timer.cancel()

    def set_offboard_control_mode(self,position: bool = True, velocity: bool = False, acceleration: bool = False, attitude: bool = False, body_rate: bool = False):
        self._offboard_control_mode['position'] = position
        self._offboard_control_mode['velocity'] = velocity
        self._offboard_control_mode['acceleration'] = acceleration
        self._offboard_control_mode['attitude'] = attitude
        self._offboard_control_mode['body_rate'] = body_rate
            
        
    def publish_offboard_control_heartbeat_signal(self):
        """Publish the offboard control mode."""
        msg = OffboardControlMode()
        msg.position = self._offboard_control_mode['position']
        msg.velocity = self._offboard_control_mode['velocity'] 
        msg.acceleration = self._offboard_control_mode['acceleration'] 
        msg.attitude = self._offboard_control_mode['attitude']
        msg.body_rate = self._offboard_control_mode['body_rate'] 
        msg.timestamp = int(self.main_node.get_clock().now().nanoseconds / 1000)
        self.offboard_control_mode_publisher.publish(msg)
    #OffboardControlMode

    
    def publish_trajectory_setpoint(self, position: list=[None,None,None], velocity: list=[None,None,None], acceleration: list=[None,None,None], yaw: float = 0.0):
        """Publish the trajectory setpoint."""
        msg = TrajectorySetpoint()
        msg.position = position
        msg.velocity = velocity
        msg.acceleration = acceleration
        msg.yaw = yaw
        msg.timestamp = int(self.main_node.get_clock().now().nanoseconds / 1000)
        self.trajectory_setpoint_publisher.publish(msg)

    def publish_position_setpoint(self, x: float, y: float, z: float):
        """Publish the trajectory setpoint."""
        msg = TrajectorySetpoint()
        msg.position = [x, y, z]
        msg.yaw = 1.57079  # (90 degree)
        msg.timestamp = int(self.main_node.get_clock().now().nanoseconds / 1000)
        self.trajectory_setpoint_publisher.publish(msg)
        self.main_node.main_node.get_logger().info(f"Publishing position setpoints {[x, y, z]}")



    def offboard_control_mode_timer_callback(self) -> None:
        """Callback function for the timer."""
        self.publish_offboard_control_heartbeat_signal()

